{
 "metadata": {
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.7-final"
  },
  "orig_nbformat": 2,
  "kernelspec": {
   "name": "python3",
   "display_name": "Python 3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2,
 "cells": [
  {
   "source": [
    "# 04 使用简单躯干规划器和逆动力学控制器\n",
    "\n",
    "用逆动力学控制器替换简单控制器，并连接简单躯干规划器。\n",
    "\n"
   ],
   "cell_type": "markdown",
   "metadata": {}
  },
  {
   "source": [
    "## 重复加载机器人的过程"
   ],
   "cell_type": "markdown",
   "metadata": {}
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "import importlib\n",
    "import sys\n",
    "from urllib.request import urlretrieve\n",
    "\n",
    "server_args = []\n",
    "\n",
    "# Start a single meshcat server instance to use for the remainder of this notebook.\n",
    "from meshcat.servers.zmqserver import start_zmq_server_as_subprocess\n",
    "proc, zmq_url, web_url = start_zmq_server_as_subprocess(server_args=server_args)\n",
    "\n",
    "# Determine if this notebook is currently running as a notebook or a unit test.\n",
    "from IPython import get_ipython\n",
    "running_as_notebook = get_ipython() and hasattr(get_ipython(), 'kernel')\n",
    "\n",
    "# Imports\n",
    "import numpy as np\n",
    "import pydot\n",
    "from ipywidgets import Dropdown, Layout\n",
    "from IPython.display import display, HTML, SVG\n",
    "\n",
    "from pydrake.all import (\n",
    "    AddMultibodyPlantSceneGraph, ConnectMeshcatVisualizer, DiagramBuilder, \n",
    "    FindResourceOrThrow, GenerateHtml, InverseDynamicsController, \n",
    "    MultibodyPlant, Parser, Simulator,\n",
    "    # add for ground model\n",
    "    RigidTransform, CoulombFriction, HalfSpace,\n",
    "    # add for single trunk model\n",
    "    GeometryFrame, Box, Sphere, GeometryInstance, MakePhongIllustrationProperties\n",
    ")\n",
    "from pydrake.multibody.jupyter_widgets import MakeJointSlidersThatPublishOnCallback"
   ]
  },
  {
   "source": [
    "导入控制器和规划器"
   ],
   "cell_type": "markdown",
   "metadata": {}
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "sys.path.append(\"/mnt/c/Users/ahl19/Documents/my_gitRepos/controller4quadruped/anymal_control\")\n",
    "\n",
    "from controllers import MPTCController, IDController, BasicController\n",
    "\n",
    "from planners import BasicTrunkPlanner, TowrTrunkPlanner"
   ]
  },
  {
   "source": [
    "仿真过程的设置变量"
   ],
   "cell_type": "markdown",
   "metadata": {}
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [],
   "source": [
    "use_lcm = False\n",
    "\n",
    "sim_time = 6.0\n",
    "dt = 5e-3\n",
    "target_realtime_rate = 1.0"
   ]
  },
  {
   "source": [
    "加载机器人和地面，同时加载逆动力学控制器模块。"
   ],
   "cell_type": "markdown",
   "metadata": {}
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [
    {
     "output_type": "stream",
     "name": "stdout",
     "text": [
      "Connecting to meshcat-server at zmq_url=tcp://127.0.0.1:6001...\nYou can open the visualizer by visiting the following URL:\nhttp://127.0.0.1:7001/static/\nConnected to meshcat-server.\n"
     ]
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='torso_to_abduct_fl_j', layout=Layout(width=\"'200'\"), max=1.6, min=-1.6, st…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "9365fdaf8ca04c5693071be8c545a025"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='abduct_fl_to_thigh_fl_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6,…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "0733c44f070641bc86badf040fbd9e10"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='thigh_fl_to_knee_fl_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6, s…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "27ed8866ff8d42bf9374787b2893f0c2"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='torso_to_abduct_fr_j', layout=Layout(width=\"'200'\"), max=1.6, min=-1.6, st…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "77245789dd1242b39d580328c5522f27"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='abduct_fr_to_thigh_fr_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6,…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "c3df8f400c854faa866385aee59bfc71"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='thigh_fr_to_knee_fr_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6, s…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "f6c12555cca1486e9f652e5329c927e2"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='torso_to_abduct_hl_j', layout=Layout(width=\"'200'\"), max=1.6, min=-1.6, st…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "a9b4672541584af18a1e57ed94105ed4"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='abduct_hl_to_thigh_hl_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6,…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "90c7ed2ce8cc4f91a58b9de9b656a602"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='thigh_hl_to_knee_hl_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6, s…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "d29e8bfb8e9c4cf093429a731c11be29"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='torso_to_abduct_hr_j', layout=Layout(width=\"'200'\"), max=1.6, min=-1.6, st…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "0b98c9ff540144589f5706e5acde45aa"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='abduct_hr_to_thigh_hr_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6,…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "627b2e1bf951484e9350975720db73eb"
      }
     },
     "metadata": {}
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "FloatSlider(value=0.0, description='thigh_hr_to_knee_hr_j', layout=Layout(width=\"'200'\"), max=2.6, min=-2.6, s…",
      "application/vnd.jupyter.widget-view+json": {
       "version_major": 2,
       "version_minor": 0,
       "model_id": "bf7e0c007b594e11ae431ecf7d786ada"
      }
     },
     "metadata": {}
    }
   ],
   "source": [
    "builder = DiagramBuilder()\n",
    "\n",
    "plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=5e-3)\n",
    "\n",
    "#Parser(plant, scene_graph).AddModelFromFile(\"../models/mini_cheetah/mini_cheetah_mesh.urdf\")\n",
    "quad = Parser(plant, scene_graph).AddModelFromFile(\"/mnt/c/Users/ahl19/Documents/my_gitRepos/controller4quadruped/models/mini_cheetah/mini_cheetah_mesh.urdf\")\n",
    "\n",
    "# Add a flat ground with friction\n",
    "X_BG = RigidTransform()\n",
    "surface_friction = CoulombFriction(\n",
    "        static_friction = 1.0,\n",
    "        dynamic_friction = 1.0)\n",
    "plant.RegisterCollisionGeometry(\n",
    "        plant.world_body(),      # the body for which this object is registered\n",
    "        X_BG,                    # The fixed pose of the geometry frame G in the body frame B\n",
    "        HalfSpace(),             # Defines the geometry of the object\n",
    "        \"ground_collision\",      # A name\n",
    "        surface_friction)        # Coulomb friction coefficients\n",
    "plant.RegisterVisualGeometry(\n",
    "        plant.world_body(),\n",
    "        X_BG,\n",
    "        HalfSpace(),\n",
    "        \"ground_visual\",\n",
    "        np.array([0.5,0.5,0.5,0.0]))    # Color set to be completely transparent\n",
    "\n",
    "# Turn off gravity\n",
    "#g = plant.mutable_gravity_field()\n",
    "#g.set_gravity_vector([0,0,0])\n",
    "\n",
    "plant.Finalize()\n",
    "assert plant.geometry_source_is_registered()\n",
    "\n",
    "####\n",
    "trunk_frame = GeometryFrame(\"trunk\")\n",
    "trunk_frame_ids = {\"trunk\":trunk_frame.id()}\n",
    "\n",
    "for foot in [\"lf\",\"rf\",\"lh\",\"rh\"]:\n",
    "    foot_frame = GeometryFrame(foot)\n",
    "    trunk_frame_ids[foot] = foot_frame.id()\n",
    "\n",
    "####\n",
    "####\n",
    "planner = builder.AddSystem(BasicTrunkPlanner(trunk_frame_ids))\n",
    "\n",
    "controller = builder.AddSystem(IDController(plant,dt,use_lcm=use_lcm))\n",
    "\n",
    "# Connect the trunk-model planner to the controller\n",
    "builder.Connect(planner.GetOutputPort(\"trunk_trajectory\"), controller.get_input_port(1))\n",
    "\n",
    "# Connect the controller to the simulated plant\n",
    "builder.Connect(controller.GetOutputPort(\"quad_torques\"),\n",
    "                plant.get_actuation_input_port(quad))\n",
    "builder.Connect(plant.get_state_output_port(),\n",
    "                controller.GetInputPort(\"quad_state\"))\n",
    "\n",
    "\n",
    "meshcat = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url=zmq_url)\n",
    "diagram = builder.Build()\n",
    "diagram_context = diagram.CreateDefaultContext()\n",
    "\n",
    "meshcat.load()\n",
    "MakeJointSlidersThatPublishOnCallback(plant, meshcat, diagram_context);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [
    {
     "output_type": "execute_result",
     "data": {
      "text/plain": [
       "<IPython.core.display.SVG object>"
      ],
      "image/svg+xml": "<svg height=\"583pt\" viewBox=\"0.00 0.00 1477.00 583.00\" width=\"1477pt\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\">\n<g class=\"graph\" id=\"graph0\" transform=\"scale(1 1) rotate(0) translate(4 579)\">\n<title>_94728064197600</title>\n<polygon fill=\"white\" points=\"-4,4 -4,-579 1473,-579 1473,4 -4,4\" stroke=\"transparent\"/>\n<g class=\"cluster\" id=\"clust1\">\n<title>cluster94728064197600diagram</title>\n<polygon fill=\"none\" points=\"8,-8 8,-567 1461,-567 1461,-8 8,-8\" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"734.5\" y=\"-551.8\">94728064197600</text>\n</g>\n<g class=\"cluster\" id=\"clust4\">\n<title>cluster94728064197600subsystems</title>\n<polygon fill=\"none\" points=\"16,-16 16,-536 1453,-536 1453,-16 16,-16\" stroke=\"white\"/>\n</g>\n<!-- 94728066073616 -->\n<g class=\"node\" id=\"node1\">\n<title>94728066073616</title>\n<polygon fill=\"none\" points=\"24,-113 24,-527 655,-527 655,-113 24,-113\" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"339.5\" y=\"-511.8\">plant</text>\n<polyline fill=\"none\" points=\"24,-504 655,-504 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"149.5\" y=\"-467.8\">geometry_query</text>\n<polyline fill=\"none\" points=\"24,-439 275,-439 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"149.5\" y=\"-402.8\">WorldModelInstance_actuation</text>\n<polyline fill=\"none\" points=\"24,-374 275,-374 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"149.5\" y=\"-337.8\">DefaultModelInstance_actuation</text>\n<polyline fill=\"none\" points=\"24,-309 275,-309 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"149.5\" y=\"-272.8\">mini_cheetah_actuation</text>\n<polyline fill=\"none\" points=\"24,-244 275,-244 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"149.5\" y=\"-207.8\">applied_generalized_force</text>\n<polyline fill=\"none\" points=\"24,-179 275,-179 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"149.5\" y=\"-142.3\">applied_spatial_force</text>\n<polyline fill=\"none\" points=\"275,-113 275,-504 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-488.8\">geometry_pose</text>\n<polyline fill=\"none\" points=\"275,-481 655,-481 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-465.8\">continuous_state</text>\n<polyline fill=\"none\" points=\"275,-458 655,-458 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-442.8\">body_poses</text>\n<polyline fill=\"none\" points=\"275,-435 655,-435 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-419.8\">spatial_velocities</text>\n<polyline fill=\"none\" points=\"275,-412 655,-412 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-396.8\">spatial_accelerations</text>\n<polyline fill=\"none\" points=\"275,-389 655,-389 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-373.8\">generalized_acceleration</text>\n<polyline fill=\"none\" points=\"275,-366 655,-366 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-350.8\">WorldModelInstance_continuous_state</text>\n<polyline fill=\"none\" points=\"275,-343 655,-343 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-327.8\">WorldModelInstance_generalized_acceleration</text>\n<polyline fill=\"none\" points=\"275,-320 655,-320 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-304.8\">DefaultModelInstance_continuous_state</text>\n<polyline fill=\"none\" points=\"275,-297 655,-297 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-281.8\">DefaultModelInstance_generalized_acceleration</text>\n<polyline fill=\"none\" points=\"275,-274 655,-274 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-258.8\">mini_cheetah_continuous_state</text>\n<polyline fill=\"none\" points=\"275,-251 655,-251 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-235.8\">mini_cheetah_generalized_acceleration</text>\n<polyline fill=\"none\" points=\"275,-228 655,-228 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-212.8\">WorldModelInstance_generalized_contact_forces</text>\n<polyline fill=\"none\" points=\"275,-205 655,-205 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-189.8\">DefaultModelInstance_generalized_contact_forces</text>\n<polyline fill=\"none\" points=\"275,-182 655,-182 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-166.8\">mini_cheetah_generalized_contact_forces</text>\n<polyline fill=\"none\" points=\"275,-159 655,-159 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-143.8\">reaction_forces</text>\n<polyline fill=\"none\" points=\"275,-136 655,-136 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-120.8\">contact_results</text>\n</g>\n<!-- 94728065650144 -->\n<g class=\"node\" id=\"node2\">\n<title>94728065650144</title>\n<polygon fill=\"none\" points=\"852.5,-458.5 852.5,-527.5 1086.5,-527.5 1086.5,-458.5 852.5,-458.5\" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"969.5\" y=\"-512.3\">scene_graph</text>\n<polyline fill=\"none\" points=\"852.5,-504.5 1086.5,-504.5 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"899.5\" y=\"-477.8\">plant_pose</text>\n<polyline fill=\"none\" points=\"946.5,-458.5 946.5,-504.5 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1016.5\" y=\"-489.3\">lcm_visualization</text>\n<polyline fill=\"none\" points=\"946.5,-481.5 1086.5,-481.5 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1016.5\" y=\"-466.3\">query</text>\n</g>\n<!-- 94728066073616&#45;&gt;94728065650144 -->\n<g class=\"edge\" id=\"edge4\">\n<title>94728066073616:y0-&gt;94728065650144:u0</title>\n<path d=\"M655,-493C738.91,-493 762.13,-481.96 841.06,-481.06\" fill=\"none\" stroke=\"black\"/>\n<polygon fill=\"black\" points=\"841.52,-484.56 851.5,-481 841.48,-477.56 841.52,-484.56\" stroke=\"black\"/>\n</g>\n<!-- 94728064184672 -->\n<g class=\"node\" id=\"node4\">\n<title>94728064184672</title>\n<polygon fill=\"none\" points=\"691,-241.5 691,-310.5 1248,-310.5 1248,-241.5 691,-241.5\" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"969.5\" y=\"-295.3\">controllers.inverse_dynamics_controller.IDController@00005627989db160</text>\n<polyline fill=\"none\" points=\"691,-287.5 1248,-287.5 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"824\" y=\"-272.3\">quad_state</text>\n<polyline fill=\"none\" points=\"691,-264.5 957,-264.5 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"824\" y=\"-249.3\">trunk_input</text>\n<polyline fill=\"none\" points=\"957,-241.5 957,-287.5 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1102.5\" y=\"-272.3\">quad_torques</text>\n<polyline fill=\"none\" points=\"957,-264.5 1248,-264.5 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1102.5\" y=\"-249.3\">output_metrics</text>\n</g>\n<!-- 94728066073616&#45;&gt;94728064184672 -->\n<g class=\"edge\" id=\"edge1\">\n<title>94728066073616:y1-&gt;94728064184672:u0</title>\n<path d=\"M655,-470C738.93,-470 614.87,-292.31 680.92,-277.04\" fill=\"none\" stroke=\"black\"/>\n<polygon fill=\"black\" points=\"681.41,-280.51 691,-276 680.69,-273.55 681.41,-280.51\" stroke=\"black\"/>\n</g>\n<!-- 94728065650144&#45;&gt;94728066073616 -->\n<g class=\"edge\" id=\"edge5\">\n<title>94728065650144:y1-&gt;94728066073616:u0</title>\n<path d=\"M1016.5,-457C1016.5,-293.31 816.36,-499.94 655,-527.5 585.84,-539.31 73.56,-576.67 23.5,-527.5 8.79,-513.05 0.73,-483.98 12.9,-474.82\" fill=\"none\" stroke=\"black\"/>\n<polygon fill=\"black\" points=\"13.89,-478.18 22.5,-472 11.92,-471.46 13.89,-478.18\" stroke=\"black\"/>\n</g>\n<!-- 94728064525936 -->\n<g class=\"node\" id=\"node5\">\n<title>94728064525936</title>\n<polygon fill=\"none\" points=\"1284,-482 1284,-528 1445,-528 1445,-482 1284,-482\" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1364.5\" y=\"-512.8\">meshcat_visualizer</text>\n<polyline fill=\"none\" points=\"1284,-505 1445,-505 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1354\" y=\"-489.8\">lcm_visualization</text>\n<polyline fill=\"none\" points=\"1424,-482 1424,-505 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1434.5\" y=\"-489.8\"> </text>\n</g>\n<!-- 94728065650144&#45;&gt;94728064525936 -->\n<g class=\"edge\" id=\"edge3\">\n<title>94728065650144:y0-&gt;94728064525936:u0</title>\n<path d=\"M1087.5,-493C1171.25,-493 1194.77,-493 1273.58,-493\" fill=\"none\" stroke=\"black\"/>\n<polygon fill=\"black\" points=\"1274,-496.5 1284,-493 1274,-489.5 1274,-496.5\" stroke=\"black\"/>\n</g>\n<!-- 94728066143280 -->\n<g class=\"node\" id=\"node3\">\n<title>94728066143280</title>\n<polygon fill=\"none\" points=\"124,-24.5 124,-93.5 555,-93.5 555,-24.5 124,-24.5\" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"339.5\" y=\"-78.3\">planners.simple.BasicTrunkPlanner@0000562798bb9430</text>\n<polyline fill=\"none\" points=\"124,-70.5 555,-70.5 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"203\" y=\"-43.8\"> </text>\n<polyline fill=\"none\" points=\"282,-24.5 282,-70.5 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"418.5\" y=\"-55.3\">trunk_trajectory</text>\n<polyline fill=\"none\" points=\"282,-47.5 555,-47.5 \" stroke=\"black\"/>\n<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"418.5\" y=\"-32.3\">trunk_geometry</text>\n</g>\n<!-- 94728066143280&#45;&gt;94728064184672 -->\n<g class=\"edge\" id=\"edge2\">\n<title>94728066143280:y0-&gt;94728064184672:u1</title>\n<path d=\"M556.5,-59C604.63,-59 624.38,-66.87 655,-104 695.89,-153.58 631.04,-242.22 680.76,-252.1\" fill=\"none\" stroke=\"black\"/>\n<polygon fill=\"black\" points=\"680.73,-255.61 691,-253 681.35,-248.64 680.73,-255.61\" stroke=\"black\"/>\n</g>\n<!-- 94728064184672&#45;&gt;94728066073616 -->\n<g class=\"edge\" id=\"edge6\">\n<title>94728064184672:y0-&gt;94728066073616:u3</title>\n<path d=\"M1249.5,-276C1265.06,-276 1259.66,-300.15 1248.5,-311 1070.81,-483.69 881.2,-469.81 691,-311 622.18,-253.53 723.82,-169.97 655,-112.5 641.53,-101.26 35.94,-100.13 23.5,-112.5 12.43,-123.51 6.23,-236.34 16.24,-267.86\" fill=\"none\" stroke=\"black\"/>\n<polygon fill=\"black\" points=\"13.63,-270.21 22.5,-276 19.17,-265.94 13.63,-270.21\" stroke=\"black\"/>\n</g>\n</g>\n</svg>"
     },
     "metadata": {},
     "execution_count": 18
    }
   ],
   "source": [
    "SVG(pydot.graph_from_dot_data(diagram.GetGraphvizString())[0].create_svg())"
   ]
  },
  {
   "source": [
    "## 运行仿真"
   ],
   "cell_type": "markdown",
   "metadata": {}
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [
    {
     "output_type": "execute_result",
     "data": {
      "text/plain": [
       "<pydrake.systems.analysis.SimulatorStatus at 0x7f0d9029d970>"
      ]
     },
     "metadata": {},
     "execution_count": 23
    }
   ],
   "source": [
    "# Simulator setup\n",
    "simulator = Simulator(diagram, diagram_context)\n",
    "if use_lcm:\n",
    "    # If we're using LCM to send messages to another simulator or a real\n",
    "    # robot, we don't want Drake to slow things down, so we'll publish\n",
    "    # new messages as fast as possible\n",
    "    simulator.set_target_realtime_rate(0.0)\n",
    "else:\n",
    "    simulator.set_target_realtime_rate(target_realtime_rate)\n",
    "    \n",
    "# Set initial states\n",
    "plant_context = diagram.GetMutableSubsystemContext(plant, diagram_context)\n",
    "\n",
    "q0 = np.asarray([ 1.0, 0.0, 0.0, 0.0,     # base orientation\n",
    "                  0.0, 0.0, 0.3,          # base position\n",
    "                  0.0, 0.0, 0.0, 0.0,     # ad/ab\n",
    "                 -0.8,-0.8,-0.8,-0.8,     # hip\n",
    "                  1.6, 1.6, 1.6, 1.6])    # knee\n",
    "qd0 = np.zeros(plant.num_velocities())\n",
    "plant.SetPositions(plant_context,q0)\n",
    "plant.SetVelocities(plant_context,qd0)\n",
    "\n",
    "\n",
    "# Run the simulation a bit\n",
    "simulator.AdvanceTo(0.01)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [
    {
     "output_type": "execute_result",
     "data": {
      "text/plain": [
       "<pydrake.systems.analysis.SimulatorStatus at 0x7f0d8203c430>"
      ]
     },
     "metadata": {},
     "execution_count": 24
    }
   ],
   "source": [
    "# Run the simulation!\n",
    "simulator.AdvanceTo(sim_time)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ]
}